#include "CSerialPort.h"
#include "Tracker.h"
#include <opencv2/videoio.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <iomanip>
#include <termios.h>
#include <netinet/in.h>
#include<sys/ioctl.h>
#include <thread>
using namespace std;
using namespace cv;

const int IMG_WIDTH = 640, IMG_HEIGHT =480;

vector<Point2f> center;
CSerialPort myserialport; //打开串口
char send_data[8];

int main(int argc, char **argv)
{
	VideoCapture video_in;
	video_in.open( "/home/ubuntu/桌面/golfbos/source/5734.avi");
	string video_name="123";
	Mat frame;

	Ptr<AKAZE> akaze = AKAZE::create(AKAZE::DESCRIPTOR_MLDB_UPRIGHT);
	akaze->setThreshold(akaze_thresh);
	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
	Tracker akaze_tracker(akaze, matcher);
	namedWindow(video_name, WINDOW_NORMAL);
	center.push_back(Point2f(640,480));
	akaze_tracker.setFirstFrame();

	Mat akaze_res;
	double Time;

	while (1)
	{	
		Time = (double)cvGetTickCount();
		send_data[0]=0;
		cout << "\r\n----------------------------------" << endl;
		video_in >> frame;
		cvtColor(frame,frame,CV_BGR2GRAY);
		if (frame.empty())
		{
			try
			{
				video_in.open(0);
			}
			catch (Exception e)
			{
				cout << e.msg << endl;
			}
			cout << "Camera LOST!" << endl;
			myserialport.writeToCom_char((char*)&send_data, sizeof(send_data));
			continue;
		}

		Mat homography = akaze_tracker.process(frame);
		cv::imshow(video_name, frame);
		if (waitKey(1) == 27)
			break;
		if (homography.empty())
		{
			myserialport.writeToCom_char((char*)&send_data, sizeof(send_data));
			 continue;
		}
		vector<Point2f> new_center;
		perspectiveTransform(center, new_center, homography);
		short x = doubleToShort(new_center[0].x - 320.0d);
		
		cout<< "\r\nx:\t" << x/3 << endl;
		send_data[0]=2;
		send_data[5] = x;
		send_data[6] = (x >> 8);
		send_data[7] = 0x5A;
		myserialport.writeToCom_char((char*)&send_data, sizeof(send_data));
		Time = (double)cvGetTickCount() - Time;                         //显示时间，单位：ms
		Time = Time / (cvGetTickFrequency() * 1000);
		cout<<"Time:"<<Time<<endl;
	}
	return 0;
}
